#pragma once
#include"common.h"
#include "FitModel.h"
#include "ImageData.h"
class MyLine3D: public CFitModel
{
public:
	MyLine3D();
	virtual ~MyLine3D(void);
	void setStartPoint(float x,float y,float z)
	{
		xa=x;ya=y;za=z;
	}
	void setEndPoint(float x,float y,float z)
	{
		xb=x;yb=y;zb=z;
	}
	double compNearDist(const MyLine3D&) const;
	void setByLine(const MyLine3D& line)
	{
		xa=line.xa;
		ya=line.ya;
		za=line.za;

		xb=line.xb;
		yb=line.yb;
		zb=line.zb;

		theta=line.theta;
		mRadius=line.mRadius;
	}
	Voxel getStartPoint() const
	{
		return Voxel(xa,ya,za);
	}
	Voxel getEndPoint() const
	{
		return Voxel(xb,yb,zb);
	}
	Voxel getHalfPoint() const
	{
		return Voxel((xa+xb)/2,(ya+yb)/2,(za+zb)/2);
	}
	Voxel getDirect() const
	{
		return Voxel(xa-xb,ya-yb,za-zb);
	}
	double compAngle(const MyLine3D& line) const;
	bool isSimilar(const MyLine3D& line);
	double getSimilarity(const MyLine3D& line);
	void adjustLine(const std::vector<cv::Point3i>& points,const CImageData& img3D);
	void kmeanLines(const std::vector<cv::Point3i>& points,const CImageData& img3D,bool  keepVoxels);//using kmeans to generate more lines
	void sampleLine(CImageData& img3D);
	void createLine(int x,int y,int z);
	double compDist(float x,float y,float z) const;
	double compDirectDist(float x,float y,float z) const;
	double compAngle(float tx,float ty,float tz) const;
	double compEnergy(int x,int y,int z) const;
	void print() const{ printf("(%.2f %.2f %.2f), (%.2f %.2f %.2f), sigma=%.2f\n",xa,ya,za,xb,yb,zb,theta);}
	double getLength() const { return sqrt(0.0+(xa-xb)*(xa-xb)+(ya-yb)*(ya-yb)+(za-zb)*(za-zb)); }
	bool  inLineRange(int x,int y,int z);
	cv::Point3f getProjection(float x,float y,float z) const;
public:
	static MyLine3D getLineByPoints(const std::vector<cv::Point3i>& cpoints,bool quickextend);
	bool validLine(){return (int)xa!=(int)xb||(int)ya!=(int)yb||(int)za!=(int)zb; }
private:
	static bool sameDirect(const Voxel& va,const Voxel& vb,const CImageData& img3D);//{ 	return getAngle(cv::Point3f(xa,ya,za),cv::Point3f(xb,yb,zb)) >0.8 ; 	}
	int guessRadius(const std::vector<cv::Point3i>& points);
	//void adjustByPoints(const std::vector<cv::Point3i>& points);
	std::vector<cv::Point3i> purgePoints(const std::vector<cv::Point3i>& points);
	cv::Mat doKmeansPoints(const cv::Mat& data,const int K);
	static MyLine3D extendLineByPoints(const std::vector<cv::Point3i>& points,const cv::Vec6f& param);
	static MyLine3D quickExtendLineByPoints(const std::vector<cv::Point3i>& points,const cv::Vec6f& param);
	bool inRange(const Voxel& vo){	return vo.x>=0&&vo.x<W&&vo.y>=0&&vo.y<H&&vo.s>=0&&vo.s<S;}
public:
	int realPos;
	double theta;
	double mRadius;
	double goodness;
	static bool finiteSearch;
	static bool exhaustiveSearch;
	std::vector<MyLine3D> clusterLines;//line generated by kmeans
	std::vector<cv::Point3i> supportPoints;//points belongs to this line
	std::vector<cv::Point3i> tubePoints;//points belongs to this line

	static bool useShortLines;
////////////////3D display///////////////////////
	osg::Node* create3DBox(int id);
	osg::Node* create3DLinePoints(int id);
	void displayEnergy();
private:
	osg::Node* create3DNode();
	osg::Vec3f getBoxPos(float radius,float angle,bool up);
	void initCrossBox();
private:
	float xa,ya,za;//start
	float xb,yb,zb;//end
	osg::Vec3d nv,nu;
};

